﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;
using OpenTK;

namespace LasRendering
{
    public static class CloudPointsHelper
    {
        private const double ZERO_TOLERANCE = 1E-6d;

        private static void AbsMinMax(out double min, out double max, params double[] value)
        {
            min = double.MaxValue;
            max = double.MinValue;
            for (int i = 0; i < value.Length; i++)
            {
                double v = Math.Abs(value[i]);
                if (v < min)
                {
                    min = v;
                }
                if (v > max)
                {
                    max = v;
                }
            }
        }

        private static double CubeRoot(double d)
        {
            return Math.Pow(d, 1d / 3);
        }

        public static Matrix3x3 ComputeCovarianceMatrix(List<Vector3> vectorList)
        {
            //Covariance Matrix
            //http://en.wikipedia.org/wiki/Covariance_matrix
            //∑ = E[(X - E[X])(X - E[X])ᵗ]

            double m11 = 0, m12 = 0, m13 = 0, m22 = 0, m23 = 0, m33 = 0;
            double sumX = 0, sumY = 0, sumZ = 0;

            for (int i = 0; i < vectorList.Count; i++)
            {
                Vector3 v = vectorList[i];
                sumX += v.X;
                sumY += v.Y;
                sumZ += v.Z;
            }
            double avgX = sumX / vectorList.Count;
            double avgY = sumY / vectorList.Count;
            double avgZ = sumZ / vectorList.Count;

            for (int i = 0; i < vectorList.Count; i++)
            {
                Vector3 v = vectorList[i];
                double deltaX = v.X - avgX;
                double deltaY = v.Y - avgY;
                double deltaZ = v.Z - avgZ;

                m11 += deltaX * deltaX;
                m12 += deltaX * deltaY;
                m13 += deltaX * deltaZ;
                m22 += deltaY * deltaY;
                m23 += deltaY * deltaZ;
                m33 += deltaZ * deltaZ;
            }

            return new Matrix3x3(
                (float)m11, (float)m12, (float)m13,
                (float)m12, (float)m22, (float)m23,
                (float)m13, (float)m23, (float)m33);
        }

        public static Vector3 ComputeEigenvalues(Matrix3x3 A)
        {
            // Eigenvalue algorithm
            // http://en.wikipedia.org/wiki/Eigenvalue_algorithm
            // The spectral theorem for symmetric matrices states that if A is a real symmetric n-by-n matrix, 
            // then all its eigenvalues are real, and there exist n linearly independent eigenvectors for A which are mutually orthogonal.

            // Eigenvalues of 3×3 matrices
            // If
            //         ┌ a b c ┐
            //     A = | d e f |
            //         └ g h i ┘
            // then the characteristic polynomial of A is
            //                       ┌ a-λ  b   c  ┐
            //     det[A - λI] = det |  d  e-λ  f  | = -λ³ + (a+e+i)λ² + (bd+gc+fh-ae-ai-ei)λ + (aei-afh-bdi+dch+gbf-gce)
            //                       └  g   h  i-λ ┘

            // In standard form of cubic function:
            //     ax³ + bx² + cx + d = 0
            // let a = 1
            //     b = -(a+e+i)
            //     c = -(bd+gc+fh-ae-ai-ei)
            //     d = -(aei-afh-bdi+dch+gbf-gce)
            double b = -(A.v11 + A.v22 + A.v33);
            double c = -(A.v12 * A.v21 + A.v13 * A.v31 + A.v23 * A.v32 - A.v11 * A.v22 - A.v11 * A.v33 - A.v22 * A.v33);
            double d = -Matrix3x3.det(A);

            // Solve the cubic equation
            // http://en.wikipedia.org/wiki/Cubic_equation
            // Reduction to a monic trinomial
            //     t³ + pt + q = 0
            double p = (3 * c - b * b) / 3;
            double q = (2 * b * b * b - 9 * b * c + 27 * d) / 27;
            double t0, t1, t2;

            if (p == 0)
            {
                // t³ + q = 0
                t0 = t1 = t2 = CubeRoot(-q);
            }
            else
            {
                // Trigonometric (and hyperbolic) method
                double acosArg = (3 * q / (2 * p)) * Math.Sqrt(-3 / p);
                if (acosArg > 1)
                {
                    acosArg = 1;
                }
                else if (acosArg < -1)
                {
                    acosArg = -1;
                }
                double theta = Math.Acos(acosArg);
                double k = 2 * Math.Sqrt(-p / 3);
                t0 = k * Math.Cos(theta / 3);
                t2 = -k * Math.Cos((Math.PI - theta) / 3);
                t1 = -t0 - t2;
                // t0 >= t1 >= t2
                Debug.Assert(t0 >= t1);
                Debug.Assert(t1 >= t2);
            }

            double x0 = t0 - b / 3;
            double x1 = t1 - b / 3;
            double x2 = t2 - b / 3;

            return new Vector3((float)x0, (float)x1, (float)x2);
        }

        public static Vector3 ComputeEigenvector(Matrix3x3 A, float eigenvalue)
        {
            // B = A - λI
            double b11 = A.v11, b12 = A.v12, b13 = A.v13;
            double b21 = A.v21, b22 = A.v22, b23 = A.v23;
            double b31 = A.v31, b32 = A.v32, b33 = A.v33;
            b11 -= eigenvalue;
            b22 -= eigenvalue;
            b33 -= eigenvalue;

            // Find max of min value in a column, swap that column with 2nd column, to avoid divide 0 later.
            double c1min, c2min, c3min;
            double c1max, c2max, c3max;
            AbsMinMax(out c1min, out c1max, b11, b21, b31);
            AbsMinMax(out c2min, out c2max, b12, b22, b32);
            AbsMinMax(out c3min, out c3max, b13, b23, b33);
            double maxOfColumnMin = c2min;
            int swapWithColumn2 = 2;
            if (c1min > maxOfColumnMin)
            {
                maxOfColumnMin = c1min;
                swapWithColumn2 = 1;
            }
            if (c3min > maxOfColumnMin)
            {
                maxOfColumnMin = c3min;
                swapWithColumn2 = 3;
            }
            if (maxOfColumnMin < ZERO_TOLERANCE)
            {
                if (c1max < ZERO_TOLERANCE)
                {
                    return new Vector3(1, 0, 0);
                }
                if (c2max < ZERO_TOLERANCE)
                {
                    return new Vector3(0, 1, 0);
                }
                if (c3max < ZERO_TOLERANCE)
                {
                    return new Vector3(0, 0, 1);
                }
                if (Math.Abs(b12) < ZERO_TOLERANCE && Math.Abs(b13) < ZERO_TOLERANCE)
                {
                    double r2 = b22 * b22 + b23 * b23;
                    double r3 = b32 * b32 + b33 * b33;
                    if (r2 > r3)
                    {
                        r2 = Math.Sqrt(r2);
                        return new Vector3(0, (float)(-b23 / r2), (float)(b22 / r2));
                    }
                    else
                    {
                        r3 = Math.Sqrt(r3);
                        return new Vector3(0, (float)(b33 / r3), (float)(-b32 / r3));
                    }
                }
                if (Math.Abs(b21) < ZERO_TOLERANCE && Math.Abs(b23) < ZERO_TOLERANCE)
                {
                    double r1 = b11 * b11 + b13 * b13;
                    double r3 = b31 * b31 + b33 * b33;
                    if (r1 > r3)
                    {
                        r1 = Math.Sqrt(r1);
                        return new Vector3((float)(-b13 / r1), 0, (float)(b11 / r1));
                    }
                    else
                    {
                        r3 = Math.Sqrt(r3);
                        return new Vector3((float)(b33 / r3), 0, (float)(-b31 / r3));
                    }
                }
                if (Math.Abs(b31) < ZERO_TOLERANCE && Math.Abs(b32) < ZERO_TOLERANCE)
                {
                    double r1 = b11 * b11 + b12 * b12;
                    double r2 = b21 * b21 + b22 * b22;
                    if (r1 > r2)
                    {
                        r1 = Math.Sqrt(r1);
                        return new Vector3((float)(-b12 / r1), (float)(b11 / r1), 0);
                    }
                    else
                    {
                        r2 = Math.Sqrt(r2);
                        return new Vector3((float)(b22 / r2), (float)(-b21 / r2), 0);
                    }
                }
            }

        START_SWAP_COLUMN:
            int retryCount = 0;
            if (swapWithColumn2 == 1)
            {
                MathHelper.Swap(ref b11, ref b12);
                MathHelper.Swap(ref b21, ref b22);
                MathHelper.Swap(ref b31, ref b32);
            }
            else if (swapWithColumn2 == 3)
            {
                MathHelper.Swap(ref b13, ref b12);
                MathHelper.Swap(ref b23, ref b22);
                MathHelper.Swap(ref b33, ref b32);
            }

            // According to definition of eigenvalue and eigenvector:
            //                        ┌ b11 b12 b13 ┐   ┌ a ┐
            //     (A - λI)*v = B*v = | b21 b22 b23 | * | b | = 0
            //                        └ b31 b32 b33 ┘   └ c ┘
            // Then the equation is
            //     b11 * a + b12 * b + b13 * c = 0
            //     b21 * a + b22 * b + b23 * c = 0
            //     b31 * a + b32 * b + b33 * c = 0
            // Let c = 1
            //     b11/b12 * a + b = -b13/b12
            //     b21/b22 * a + b = -b23/b22
            //     b31/b32 * a + b = -b33/b32
            // Using Least square method:
            // http://en.wikipedia.org/wiki/Least_squares
            //     For Y = αX + β
            //       α = (N*∑XY - ∑X∑Y) / (N*∑(X^2) - (∑X)^2)
            //       β = (∑Y)/N - α * (∑X)/N
            double a, b, c;
            double x, y;
            double sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0;
            x = b11 / b12;
            y = -b13 / b12;
            sumX += x;
            sumY += y;
            sumXY += x * y;
            sumX2 += x * x;

            x = b21 / b22;
            y = -b23 / b22;
            sumX += x;
            sumY += y;
            sumXY += x * y;
            sumX2 += x * x;

            x = b31 / b32;
            y = -b33 / b32;
            sumX += x;
            sumY += y;
            sumXY += x * y;
            sumX2 += x * x;

            a = (3 * sumXY - sumX * sumY) / (3 * sumX2 - sumX * sumX);
            b = sumY / 3 - a * sumX / 3;
            c = 1;

            if (double.IsNaN(a) || double.IsNaN(b) && retryCount < 3)
            {
                swapWithColumn2 = swapWithColumn2 % 3 + 1;
                retryCount++;
                goto START_SWAP_COLUMN;
            }

            Debug.Assert(!double.IsNaN(a));
            Debug.Assert(!double.IsNaN(b));
            Debug.Assert(!double.IsNaN(c));

            double length = Math.Sqrt(a * a + b * b + c * c);
            a /= length;
            b /= length;
            c /= length;

            // Swap back
            Vector3 eigenvector;
            if (swapWithColumn2 == 1)
            {
                eigenvector = new Vector3((float)b, (float)a, (float)c);
            }
            else if (swapWithColumn2 == 3)
            {
                eigenvector = new Vector3((float)a, (float)c, (float)b);
            }
            else
            {
                eigenvector = new Vector3((float)a, (float)b, (float)c);
            }

            return eigenvector;
        }

        public static Vector3 ComputeEigenvectorOfMinEigenvalue(List<Vector3> vertexList)
        {
            var M = CloudPointsHelper.ComputeCovarianceMatrix(vertexList);

            var eigenvalues = CloudPointsHelper.ComputeEigenvalues(M);
            return CloudPointsHelper.ComputeEigenvector(M, eigenvalues.Z);
        }

        public static Vector3 ComputeEigenvectorOfMaxEigenvalue(List<Vector3> vertexList)
        {
            var M = CloudPointsHelper.ComputeCovarianceMatrix(vertexList);

            var eigenvalues = CloudPointsHelper.ComputeEigenvalues(M);
            return CloudPointsHelper.ComputeEigenvector(M, eigenvalues.X);
        }

        public static float ComputeWeightOfMinEigenvalue(List<Vector3> vertexList)
        {
            var M = CloudPointsHelper.ComputeCovarianceMatrix(vertexList);
            var eigenvalues = CloudPointsHelper.ComputeEigenvalues(M);
            float sum = Math.Abs(eigenvalues.X) + Math.Abs(eigenvalues.Y) + Math.Abs(eigenvalues.Z);
            return Math.Abs(eigenvalues.Z) / sum;
        }

        public static void LineFitting(List<Vector3> sortedPositionList, out Vector3 endpoint1, out Vector3 endpoint2)
        {
            Vector3 centroid = new Vector3(0, 0, 0);

            for (int i = 0; i < sortedPositionList.Count; i++) // Loop through List with for
            {
                centroid += sortedPositionList[i];
            }

            centroid /= sortedPositionList.Count;

            var M = CloudPointsHelper.ComputeCovarianceMatrix(sortedPositionList);
            var eigenvalues = CloudPointsHelper.ComputeEigenvalues(M);
            
            var eigenvector = CloudPointsHelper.ComputeEigenvector(M, eigenvalues.X);   // the maximum eigenvalue

            endpoint1 = centroid + Vector3.Dot(sortedPositionList[0] - centroid, eigenvector) * eigenvector;
            endpoint2 = centroid + Vector3.Dot(sortedPositionList[sortedPositionList.Count - 1] - centroid, eigenvector) * eigenvector;
        }
    }
}
